
#include "tracker.h"
#include <iostream>

using namespace std;

namespace eqPc
{

Tracker::Tracker()
        : _running( false ),
          _worldToEmitter( eq::Matrix4f::IDENTITY ),
          _sensorToObject( eq::Matrix4f::IDENTITY )
{

}


bool Tracker::init( const std::string& port )
{
	std::stringstream ss(port); 
  
	int i;
	ss >> i;
	std::cout <<"Initializing Tracker (Port: "<< i << ")"<<std::endl;

	//initialize Tracker
	dtrack = new DTrackSDK(i);

	if(!dtrack->isLocalDataPortValid() || !dtrack->receive()){
		std::cout << "DTrack init error" << std::endl;
		return false;
	}

	_running=true;
	head = new Head();
	flyStick = new FlyStick();
	return true;

}

bool Tracker::update()
{	
   if( !_running )
   {
      std::cout << "Update error, tracker not running" << std::endl;
      return false;
   }
   else
      return _update();
}

bool Tracker::_update()
{
    
	if(dtrack->receive())
	{
		// bodies:
		DTrack_Body_Type_d body;
		for(int i=0; i<dtrack->getNumBody(); i++){
			body = *dtrack->getBody(i);
			_updateHead(body);
		}

		//flysticks:
		DTrack_FlyStick_Type_d flystick;
		for(int i=0; i<dtrack->getNumFlyStick(); i++){
			flystick = *dtrack->getFlyStick(i);
			_updateFlyStick(flystick);
		}

	}else{
		_errorToConsole();
		return false;	
	
	}
}

eq::Matrix4f& Tracker::getTransformationMatrix(BodyType body)
{
	
	if(body==BodyType::HEAD){
		return _worldToEmitter * head->getTransformation() * _sensorToObject;
	}
	else if(body==BodyType::FLYSTICK)
		return _worldToEmitter * flyStick->getTransformation() * _sensorToObject;
}

const eq::Matrix4f& Tracker::getDeltaTransformation(BodyType body, bool relativeToWorld)
{
	if(body==BodyType::FLYSTICK){
		if(!relativeToWorld)
			return flyStick->getDeltaMatrix();
		else
			return flyStick->getDeltaMatrix() * _worldTriggerMatrix;
	}

}


void Tracker::_updateHead(DTrack_Body_Type_d &body)
{
	eq::Matrix4f t = eq::Matrix4f::IDENTITY;

	//set translation
	eq::Vector3f pos(body.loc[0]/1000,body.loc[1]/1000,body.loc[2]/1000);
	t.set_translation( pos );

	//set rotation
	t.array[0] = body.rot[0]; t.array[4] = body.rot[3]; t.array[8] = body.rot[6];
	t.array[1] = body.rot[1]; t.array[5] = body.rot[4]; t.array[9] = body.rot[7];
	t.array[2] = body.rot[2]; t.array[6] = body.rot[5]; t.array[10] = body.rot[8];

	//update body
	head->update(t,body.quality);
}

void Tracker::_updateFlyStick(DTrack_FlyStick_Type_d &flystick)
{
	eq::Matrix4f t = eq::Matrix4f::IDENTITY;

	//set translation (scale to meters)
	eq::Vector3f pos(flystick.loc[0]/1000.0,flystick.loc[1]/1000.0,flystick.loc[2]/1000.0);
	t.set_translation( pos );

	//set rotation
	t.array[0] = flystick.rot[0]; t.array[4] = flystick.rot[3]; t.array[8] = flystick.rot[6];
	t.array[1] = flystick.rot[1]; t.array[5] = flystick.rot[4]; t.array[9] = flystick.rot[7];
	t.array[2] = flystick.rot[2]; t.array[6] = flystick.rot[5]; t.array[10] = flystick.rot[8];

	//handle buttons (0: Trigger, 1-4: Left to Right)
	int b[5];
	b[0]=flystick.button[0];
	b[1]=flystick.button[4];
	b[2]=flystick.button[3];
	b[3]=flystick.button[2];
	b[4]=flystick.button[1];

	//handle joystick ([0]:X,[1]:Y)
	float j[2]; j[0]=flystick.joystick[0];j[1]=flystick.joystick[1];

	//update body
	flyStick->update(t,flystick.quality,b,j);

}

ButtonState Tracker::getButtonState(Button button)
{
			return flyStick->getButtonState(button);
}

void Tracker::_outputToConsole(DTrack_Body_Type_d &body)
{
	if(body.quality < 0){
					cout << "bod " << body.id << " not tracked" << endl;
				}else{
					cout << "bod " << body.id << " qu " << body.quality
						 << " loc " << body.loc[0] << " " << body.loc[1] << " " << body.loc[2]
						 << " rot " << body.rot[0] << " " << body.rot[1] << " " << body.rot[2]
						 << " " << body.rot[3] << " " << body.rot[4] << " " << body.rot[5]
						 << " " << body.rot[6] << " " << body.rot[7] << " " << body.rot[8] << endl;
				}

}

void Tracker::_errorToConsole()
{
		if (dtrack->getLastDataError() == DTrackSDK::ERR_TIMEOUT) {
			cout << "--- timeout while waiting for tracking data" << endl;
		}

		if (dtrack->getLastDataError() == DTrackSDK::ERR_NET) {
			cout << "--- error while receiving tracking data" << endl;
		}

		if (dtrack->getLastDataError() == DTrackSDK::ERR_PARSE){
			cout << "--- error while parsing tracking data" << endl;
		}
}

}
